classdef KinemTrajTest < matlab.unittest.TestCase
    
    properties (TestParameter)
        genFuncName = {'gentgi_navtest_L_racetrack'};
        genFuncInArg = {{[0 1 0]', [0 0 pi/2]', [10 30 30 10], 100}}; % L型轨迹
        samplePeriod = {0.005};
        g = {9.793632}; % 重力加速度，m/s^2
        cst = {load('earthconstants_WGS84.mat')};
        lV_IMU = {0*[2000.0; 1000.0; 3000.0]}; % FIXME: 杆臂较大时nav和trajGen的差值较大
        lV_displm = {0*[5; 10; 20]};
    end
    
    methods (Test)
        function vldKinemTraj(testCase, genFuncName, genFuncInArg, samplePeriod, g, cst, lV_IMU, lV_displm)
            deg2Rad = pi / 180;
            
            KT = KinemTraj(samplePeriod, g, cst);
            [KT.Cfg.enableNavigation, KT.Par.IMU.lV, KT.Par.displm.lV] = deal(true, lV_IMU, lV_displm);
            KT.InitCon.pos = [30 * deg2Rad, 110 * deg2Rad, 100]'; % 纬度、经度、高度
            KT = KT.genTrajGenIn_veh(genFuncName, genFuncInArg);
            [out_trajGen, auxOut_trajGen] = KT.runTrajGen;
            
            testCase.verifyEqual(out_trajGen.CBG, auxOut_trajGen.nav.CBG, 'AbsTol', 1e-16);
            testCase.verifyEqual(out_trajGen.bodyAtt, auxOut_trajGen.nav.att, 'AbsTol', 1e-14);
            testCase.verifyEqual(out_trajGen.vG(:, :, end), auxOut_trajGen.nav.vG, 'AbsTol', 1e-12);
            testCase.verifyEqual(out_trajGen.pos(:, :, end), auxOut_trajGen.nav.pos, 'AbsTol', 1e-8);
        end
    end
end